/// 给定4个点的位置，匹配出4个点的顺序，后面是个点是通过刚性旋转移动得到的

#include <opencv2/core.hpp>
#include <iostream>
#include <memath/MeCvMath.h>

using namespace std;

/// 给定4个点的位置，匹配出4个点的顺序，后面是个点是通过刚性旋转移动得到的
class Reorder4Points {
protected:
    /// 点的特征: 点 + 和其他3点之间的距离
    struct PointFeature {
        cv::Point3d p;
        vector<double> sortedDistanceToOther;
    };

    bool isDisInErr(double &d1, double &d2)
    {
        return abs(d1-d2) < allowErr;
    }

    bool isBiggerDis(double &d, double maxD)
    {
        return d > maxD+allowErr;
    }

    bool isSmallerDis(double &d, double minD)
    {
        return d < minD-allowErr;
    }

    bool isSameDistance(std::vector<double> &a, std::vector<double> &b)
    {
        if (a.size() == b.size() && a.size()>0) {
            for (uint i=0; i<a.size(); i++) {
                if (!isDisInErr(a[i], b[i])) {
                    return false;
                }
            }
            return true;
        } else {
            return false;
        }
    }

    /// 两两之间距离计算
    vector<double> distancePairPoints(vector<cv::Point3d> &points) {
        vector<double> distances;
        int n = points.size();
        for (int i = 0; i< n-1; i++) {
            for (int j = i+1; j < n; j++) {
                double d = MeCvMath::distanceFrom2p(points[i], points[j]);
                distances.push_back(d);
            }
        }

        return distances;
    }

    /// 生成4个点的特征
    vector<PointFeature> generate4PointFeature(vector<cv::Point3d> &points) {
        if (points.size() != 4) {
            return vector<PointFeature>();
        }
        vector<double> distances = distancePairPoints(points);
        // distance: 0   1   2   3   4   5
        //           01  02  03  12  13  23

        vector<PointFeature> pointFeatures(4);

        // 0  - 1 2 3
        pointFeatures[0].p = points[0];
        pointFeatures[0].sortedDistanceToOther.push_back(distances[0]);
        pointFeatures[0].sortedDistanceToOther.push_back(distances[1]);
        pointFeatures[0].sortedDistanceToOther.push_back(distances[2]);
        sort(pointFeatures[0].sortedDistanceToOther.begin(), pointFeatures[0].sortedDistanceToOther.end());

        pointFeatures[1].p = points[1];
        pointFeatures[1].sortedDistanceToOther.push_back(distances[0]);
        pointFeatures[1].sortedDistanceToOther.push_back(distances[3]);
        pointFeatures[1].sortedDistanceToOther.push_back(distances[4]);
        sort(pointFeatures[1].sortedDistanceToOther.begin(), pointFeatures[1].sortedDistanceToOther.end());

        pointFeatures[2].p = points[2];
        pointFeatures[2].sortedDistanceToOther.push_back(distances[1]);
        pointFeatures[2].sortedDistanceToOther.push_back(distances[3]);
        pointFeatures[2].sortedDistanceToOther.push_back(distances[5]);
        sort(pointFeatures[2].sortedDistanceToOther.begin(), pointFeatures[2].sortedDistanceToOther.end());

        pointFeatures[3].p = points[3];
        pointFeatures[3].sortedDistanceToOther.push_back(distances[2]);
        pointFeatures[3].sortedDistanceToOther.push_back(distances[4]);
        pointFeatures[3].sortedDistanceToOther.push_back(distances[5]);
        sort(pointFeatures[3].sortedDistanceToOther.begin(), pointFeatures[3].sortedDistanceToOther.end());

        return pointFeatures;
    }

    /// 以A为基准，将B里面的点一一对应上去
    bool map4Points(vector<cv::Point3d> &points_A, vector<cv::Point3d> &points_B) {

        // 生成4点结构， 确保4点距离
        vector<PointFeature> ptsFtsA = generate4PointFeature(points_A);
        vector<PointFeature> ptsFtsB = generate4PointFeature(points_B);
        if (ptsFtsA.size() != 4 || ptsFtsB.size() != 4) {
            cerr << "The count of points is not equal 4." << endl;
            return false;
        }

        if (debugLevel >= 2) {
            cout << "A info:" << distancePairPoints(points_A) << endl;
            for (int k = 0; k < 4; k++) {
                cout << "    A: " <<  ptsFtsA[k].p.x << ", " <<  ptsFtsA[k].p.y << ", " <<  ptsFtsA[k].p.z << ", " << ptsFtsA[k].sortedDistanceToOther << endl;
            }
            cout << "B info:" << distancePairPoints(points_B) << endl;
            for (int k = 0; k < 4; k++) {
                cout << "    B: " <<  ptsFtsB[k].p.x << ", " <<  ptsFtsB[k].p.y << ", " <<  ptsFtsB[k].p.z << ", " << ptsFtsB[k].sortedDistanceToOther << endl;
            }
        }

        // 比较B中点中，距离找到不相同的点 1 - 2, 3, 4  其中2，3 相等， 可以判断出 1，4点

        vector<int> index = {-1, -1, -1, -1};
        int nHasMapped = 0;
        vector<int> indexA = {-1, -1, -1, -1};
        for (int step = 0; step < 3; step++) {
            for(int i=0; i< ptsFtsB.size(); i++) {

                int nSame = 0;
                int mappedIndex = -1;
                if (index[i] != -1) {
                    continue;
                }
                for (int j=0; j<ptsFtsA.size(); j++) {
                    if (indexA[j] == -1 && isSameDistance(ptsFtsB[i].sortedDistanceToOther, ptsFtsA[j].sortedDistanceToOther)) {
                        nSame++;
                        mappedIndex = j;
                    }
                }
                // 只能有一个
                if (nSame != 1) {
                    if (debugLevel >=1) cout << "More than 1 mapped to B at " << i << endl;
                    index[i] = -1;
                    continue;
                }
                index[i] = mappedIndex;
                indexA[mappedIndex] = i;
                nHasMapped++;
            }
            if (debugLevel >=2) {
                cout << "step:" << step <<  ", hasMapped: " << nHasMapped
                     << ", mapped: " << index << endl
                     << ", mappedA " << indexA << endl;
            }
            if (nHasMapped >= 4) {
                break;
            }
        }

        if (nHasMapped != 4) {
            return false;
        }

        // 最后验证位置， 如果不一致，返回错误
        vector<cv::Point3d> result = {points_B[indexA[0]], points_B[indexA[1]], points_B[indexA[2]], points_B[indexA[3]]};
        vector<double> dA = distancePairPoints(points_A);
        vector<double> dB = distancePairPoints(result);
        if (debugLevel >= 2) {
            cout << "distance of A: " << dA << endl;
            cout << "distance of B: " << dB << endl;
            cout << "pointsB:" << result << endl;
        }

        if (!isSameDistance(dA, dB)) {
            return false;
        }
        points_B = result;
        return true;
    }

public:
    static bool reorder4Points(vector<cv::Point3d> &points_A, vector<cv::Point3d> &points_B, double eps=0.8, int debugLevel=0) {
        Reorder4Points mapper;
        mapper.allowErr = eps;
        mapper.debugLevel = debugLevel;
        return mapper.map4Points(points_A, points_B);
    }
public:
    double allowErr = 0.8;
    int    debugLevel = 0; // 0 - no debug, 1 - small , 2 - detail
};

int main(int argc, char *argv[])
{

    vector<cv::Point3d> point_A = {
        {72, 0, 0},
        {0, 0, 0},
        {5, 84, 0},
        {75, 77, 0}
    };

    vector<cv::Point3d> point_B1 = {
        {72, 0, 0},
        {5, 84, 0},
        {0, 0, 0},
        {75, 77, 0}
    };
    vector<cv::Point3d> point_B2 = {
        {56.041817, 61.879513, 1768.094238},
        {125.382759, 76.592735, 1768.726074},
        {42.488850, 144.489655, 1752.494629},
        {114.492737, 152.492264, 1754.263550}
    };


    vector<cv::Point3d> &point_B = point_B2;
    std::cout << "point_A: " << point_A << endl;
    std::cout << "point_B: " << point_B << endl;

    bool bMapped = Reorder4Points::reorder4Points(point_A, point_B, 1.2, 2);

    if (bMapped)
        std::cout << "After mapped point_B: " << point_B << endl;
    else
        std::cout << "error when mapped B to A" << endl;

    return 0;
}
